/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.networktables.NetworkTableKeyNotDefined;

/**
 *
 * @author Jarrod Risley
 */

public class VisualTrackingSubsystem extends Subsystem {
   
    /*
     * Global Class Variables
     */
    private NetworkTable kittyNet;
    private String subsystemName;
    
    public boolean gotData = false;
    
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    protected void initDefaultCommand() {
    } // end initDefaultCommand
    
    
    // CONSTRUCTOR //
    
    
    public VisualTrackingSubsystem(String name) {
        
        System.out.println("Creating VisualTrackingSubsystem " + name + "...");
        subsystemName = name;
        
        kittyNet = NetworkTable.getTable("10.39.58.2");
        
    } // end VisualTrackingSubsystem Constructor
    
    
    // METHODS //
    
    
    /**
     * Grabs the vision information from KittyNet.
     */
    
    public void grabData() {
        
        double horizontalDistance = 0;
        double hypotenuse = 0;
        double recenteringDistance = 0;
        gotData = false;
        
        if (kittyNet.isConnected()) {
            
            
            try {
                horizontalDistance = kittyNet.getDouble("horizontalDistance");
                hypotenuse = kittyNet.getDouble("hypotenuse");
                recenteringDistance = kittyNet.getDouble("recenteringDistance");
            } catch (NetworkTableKeyNotDefined ex) {
                ex.printStackTrace();
            } // end try-catch
            
            System.out.println("HorizontalDistance: " + horizontalDistance);
            System.out.println("Hypotenuse: " + hypotenuse);
            System.out.println("Recentering Distance: " + recenteringDistance);
            gotData = true;
            
        } else {
            System.out.println("Data has not been aquired. Please contact "
                    + "network administrator, also known as Jarrod Risley, "
                    + "should he have any idea what is happening.");
        } // end if-else
        
    } // end grabData
    
} // end VisualTrackingSubsystem 2.0
